//-*- C++ -*-
// Copyright 2008 Isis Innovation Limited
//
// This header declares the Tracker class.
// The Tracker is one of main components of the system,
// and is responsible for determining the pose of a camera
// from a video feed. It uses the Map to track, and communicates 
// with the MapMaker (which runs in a different thread)
// to help construct this map.
//
// Initially there is no map, so the Tracker also has a mode to 
// do simple patch tracking across a stereo pair. This is handled 
// by the TrackForInitialMap() method and associated sub-methods. 
// Once there is a map, TrackMap() is used.
//
// Externally, the tracker should be used by calling TrackFrame()
// with every new input video frame. This then calls either 
// TrackForInitialMap() or TrackMap() as appropriate.
//

#ifndef __TRACKER_H
#define __TRACKER_H

#include "MapMaker.h"
#include "ATANCamera.h"
#include "MiniPatch.h"
#include "Relocaliser.h"

#include <sstream>
#include <vector>
#include <list>

namespace PTAM {

//In which feature level should the trails be computed during initialization
#define TRAIL_LEVEL 1

class TrackerData;

struct Trail    // This struct is used for initial correspondences of the first stereo pair.
{
    MiniPatch mPatch;
    CVD::ImageRef irCurrentPos;
    CVD::ImageRef irInitialPos;
};

class Tracker {
public:
    Tracker(CVD::ImageRef irVideoSize, const ATANCamera& camera, Map& map, MapMaker& mm);
    ~Tracker();

    // TrackFrame is the main working part of the tracker: call this every frame.
    void TrackFrame(CVD::Image<CVD::byte>& imFrame);

    inline SE3<> GetCurrentPose() { return mse3CamFromWorld; }

    // Gets messages to be printed on-screen for the user.
    std::string GetMessageForUser();
    void Reset();                   // Restart from scratch. Also tells the mapmaker to reset itself.
    void PokeTracker();
    std::list<Trail> getTrails();
    KeyFrame currentKeyFrame();
    double lastTrackingTime();
    float trackingQuality();
    SE3<> cameraPose();

protected:
    KeyFrame mCurrentKF;            // The current working frame as a keyframe struct

    // The major components to which the tracker needs access:
    Map& mMap;                      // The map, consisting of points and keyframes
    MapMaker& mMapMaker;            // The class which maintains the map
    ATANCamera mCamera;             // Projection model
    Relocaliser mRelocaliser;       // Relocalisation module

    CVD::ImageRef mirSize;          // Image size of whole image

    // The following members are used for initial map tracking (to get the first stereo pair and correspondences):
    void TrackForInitialMap();      // This is called by TrackFrame if there is not a map yet.
    enum {
        TRAIL_TRACKING_NOT_STARTED,
        TRAIL_TRACKING_STARTED,
        TRAIL_TRACKING_COMPLETE
    } mnInitialStage;  // How far are we towards making the initial map?
    void TrailTracking_Start();     // First frame of initial trail tracking. Called by TrackForInitialMap.
    int TrailTracking_Advance();   // Steady-state of initial trail tracking. Called by TrackForInitialMap.
    std::list<Trail> mlTrails;      // Used by trail tracking
    KeyFrame mFirstKF;              // First of the stereo pair
    KeyFrame mPreviousFrameKF;      // Used by trail tracking to check married matches

    // Methods for tracking the map once it has been made:
    void TrackMap();                // Called by TrackFrame if there is a map.
    void AssessTrackingQuality();   // Heuristics to choose between good, poor, bad.
    void ApplyMotionModel();        // Decaying velocity motion model applied prior to TrackMap
    void UpdateMotionModel();       // Motion model is updated after TrackMap
    int SearchForPoints(std::vector<TrackerData*>& vTD,
                        int nRange,
                        int nFineIts,
                        int start = 0,
                        int end = -1);  // Finds points in the image
    static void SearchForPointsThreaded(void*);
    static void* SearchForPointsThreaded2(void*);
    Vector<6> CalcPoseUpdate(std::vector<TrackerData*> vTD,
                             double dOverrideSigma = 0.0,
                             bool bMarkOutliers = false); // Updates pose from found points.
    SE3<> mse3CamFromWorld;           // Camera pose: this is what the tracker updates every frame.
    SE3<> mse3StartPos;               // What the camera pose was at the start of the frame.
    Vector<6> mv6CameraVelocity;    // Motion model
    double mdVelocityMagnitude;     // Used to decide on coarse tracking
    double mdMSDScaledVelocityMagnitude; // Velocity magnitude scaled by relative scene depth.
    bool mbDidCoarse;               // Did tracking use the coarse tracking stage?

    bool mbDraw;                    // Should the tracker draw anything to OpenGL?

    // Interface with map maker:
    int mnFrame;                    // Frames processed since last reset
    int mnLastKeyFrameDropped;      // Counter of last keyframe inserted.
    void AddNewKeyFrame();          // Gives the current frame to the mapmaker to use as a keyframe

    // Tracking quality control:
    int manMeasAttempted[LEVELS];
    int manMeasFound[LEVELS];
    enum {
        BAD, DODGY, GOOD
    } mTrackingQuality;
    int mnLostFrames;

    std::vector<TrackerData*> mAttempted;

    // Relocalisation functions:
    bool AttemptRecovery();         // Called by TrackFrame if tracking is lost.
    bool mbJustRecoveredSoUseCoarse;// Always use coarse tracking after recovery!

    // Frame-to-frame motion init:
    SmallBlurryImage* mpSBILastFrame;
    SmallBlurryImage* mpSBIThisFrame;
    void CalcSBIRotation();
    Vector<6> mv6SBIRot;
    bool mbUseSBIInit;

    // User interaction for initial tracking:
    bool mbUserPressedSpacebar;
    std::ostringstream mMessageForUser;

    bool updateOnlyPosition;

    Mutex finderlock;
    int threadsdonecount;
    CondVar threaddone;
    ThreadPool threadpool;

    float mdTotalFracFound;
    float mdLargeFracFound;

    float curFPS = 0;
    double lastFPSTime;
    double mLastTrackingTime;

    int numGoodFrames;
};

struct TrackerThreadStarter {
    Tracker* context;
    std::vector<TrackerData*>* vTD;
    int nRange;
    int nFineIts;
    int start;
    int end;
};


}


#endif






